#include <msp430x26x.h>
#include "../Hardware/PortDef.h"
#include "../Utilities/Delay.h"
#include "../Communication/SPI.h"
#include "Gyro.h"

// Control command defines
#define COMMAND_CONV_Z    0x8300 //1000001100000000
#define COMMAND_CONV_TEMP 0x8700 //1000011100000000

// Private functions
static void sendCommand(unsigned int command);
static int getValue(void);

/*
 * This is the initialization routine for the Z axis gyro.  There
 * is none that I could find in the datasheet.  Any future mod that
 * has some startup work could have code in here.
 */
void GyroInit(void)
{
	// No init?
}

/*
 * Get the Z axis gyro readings.  Enable the gyro, send the conversion command
 * for the Z axis, disable the gyro (while waiting 250ns), enable it again
 * and read out the value (then disable).  Some of these timings could be
 * adjusted based on the data sheet, but they work well and the routine
 * is pretty fast.
 * 
 * At the end we check to make sure some of the bits in the response are zero (z gyro)
 * which they should be.  If they aren't, theres some kind of error and we return
 * zero (a clear sign that somethings wrong in a terminal readout etc.).  If they
 * are then we sign extend and return.
 */
int GyroGetZ(void)
{
	int temp;
	
	ENABLE_GYRO();
	sendCommand(COMMAND_CONV_Z);
	DISABLE_GYRO();
	DELAY_250NS();
	ENABLE_GYRO();
	temp = getValue();
	DISABLE_GYRO();
	
	// Sanity check on output (should return bits saying Gyro out converted)
	if ((temp & 0x3000) == 0x0000)
		return (temp << 4) >> 4;	// Sign extend result
	else
		return 0;
}

/*
 * Get the gyro temperature readings.  Enable the gyro, send the conversion command
 * for the temperature, disable the gyro (while waiting 250ns), enable it again
 * and read out the value (then disable).  Some of these timings could be
 * adjusted based on the data sheet, but they work well and the routine
 * is pretty fast.
 * 
 * At the end we check to make sure some of the bits in the response are one (temp)
 * which they should be.  If they aren't, theres some kind of error and we return
 * zero (a clear sign that somethings wrong in a terminal readout etc.).  If they
 * are then we sign extend and return.
 */
int GyroGetTemp(void)
{
	int temp;
	
	ENABLE_GYRO();
	sendCommand(COMMAND_CONV_TEMP);
	DISABLE_GYRO();
	DELAY_250NS();
	ENABLE_GYRO();
	temp = getValue();
	DISABLE_GYRO();
	
	// Sanity check on output (should return bits saying temp converted)
	if ((temp & 0x3000) == 0x1000)
		return (temp << 4) >> 4;
	else
		return 0;
}

/*
 * Send an arbitrary character over SPI.  This is manually bitbanged SPI
 * according to the Gyro (Z) datasheet specification.
 */
static void sendCommand(unsigned int command)
{
	unsigned char i;
	
	for (i = 16; i > 0; i--)
	{
		SCLK_HIGH();
		if (command & 0x8000) // MSB first
		{
			MOSI_HIGH();
		}
		else
		{
			MOSI_LOW();
		}
		DELAY_250NS();
		SCLK_LOW();
		DELAY_250NS();
		command <<= 1; // Put next bit in MSB
	}
}

/*
 * Receive a character over SPI.  This is manually bitbanged SPI
 * according to the Gyro (Z) datasheet specification.
 */
static int getValue(void)
{
	unsigned int temp;
	unsigned int i;
	
	// Initialize data to zero
	temp = 0x0000;
	
	MOSI_LOW(); // 0's are placeholders (0 Write does nothing to control register)
	
	for (i = 0x8000; i > 0; i>>=1)
	{
		SCLK_HIGH();
		DELAY_250NS();
		if (MISO)
		{
			temp |= i;
		}
		SCLK_LOW();
		DELAY_250NS();
	}
	
	return temp;
}
